/*
 * triangulation.cpp
 *
 *  Created on: 16/11/2011
 *      Author: Aeradon
 */

#ifdef PF2D_RENDER
	#define RENDER	RenderCallBack();
#else
	#define RENDER
#endif

#ifdef _DEBUG
	#include <fstream>
	#include <iostream>
#endif

#include "../include/triangulation.hpp"
#include "../include/constraint.hpp"
#include "../include/symedge.hpp"
#include "../include/vertex.hpp"
#include "../include/face.hpp"
#include "../include/edge.hpp"

#include <cmath>
#include <cstdio>
#include <cstdlib>

Triangulation::Triangulation(T w, T h) :
width(w),
height(h),
isMarking(false),
curMark(0)
{
	std::vector<SymEdge*> v_faceEdges;
	std::vector<SymEdge*> v_faceSym;

	T cxmin = 0;
	T cxmax = w;
	T cymin = 0;
	T cymax = h;

	Vertex* v1 = createVertex(cxmin, cymin);
	Vertex* v2 = createVertex(cxmax, cymin);
	Vertex* v3 = createVertex(cxmax, cymax);
	Vertex* v4 = createVertex(cxmin, cymax);

	// setting a boundary face in face list.
	// this _boundaryFace will be referenced to by boundary edges.
	boundaryFace = new Face();
	boundaryFace->isBoundary = true;
	v_face.push_back(boundaryFace);

	SymEdge* e1 = createSymEdge(v1,v2);
	e1->isBoundary = true;
	e1->sym()->isBoundary = true;
	e1->setFac(boundaryFace);
	v_faceEdges.push_back(e1);
	v_faceSym.push_back(e1->sym());

	SymEdge* e2 = createSymEdge(v2,v3);
	e2->isBoundary = true;
	e2->sym()->isBoundary = true;
	e2->setFac(boundaryFace);
	v_faceEdges.push_back(e2);
	v_faceSym.push_back(e2->sym());

	SymEdge* e3 = createSymEdge(v3,v4);
	e3->isBoundary = true;
	e3->sym()->isBoundary = true;
	e3->setFac(boundaryFace);
	v_faceEdges.push_back(e3);
	v_faceSym.push_back(e3->sym());

	SymEdge* e4 = createSymEdge(v4,v1);
	e4->isBoundary = true;
	e4->sym()->isBoundary = true;
	e4->setFac(boundaryFace);
	v_faceEdges.push_back(e4);
	v_faceSym.push_back(e4->sym());

	// Set Edges for the boudaryFace
	boundaryFace->setEdges(v_faceEdges);

	//Create 2 triangles to start the triangulation
	// Face* f1 = createFace(v_faceSym);
	// splitFace(f1,v3,v1);
	SymEdge* e5 = createSymEdge(v1,v3);
	createFace(e5, e2->sym(), e1->sym());
	createFace(e5->sym(),e4->sym(), e3->sym());
}

Triangulation::~Triangulation()
{
	for (Face* face : v_face)
	{
		if(face)
			delete face;
	}

	for (Edge* edge : v_edge)
	{
		if(edge)
			delete edge;
	}

	for (SymEdge* sym : v_symEdge)
	{
		if(sym)
			delete sym;
	}

	for (Vertex* vertex : v_vertex)
	{
		if(vertex)
			delete vertex;
	}

	for (Constraint* constraint : v_constraint)
	{
		if(constraint)
			delete constraint;
	}
}

// Locate Methods

// Insert Constraints
void Triangulation::insertConstraint(Constraint constraint)
{
	insertConstraint(constraint.p1.x, constraint.p1.y,
			constraint.p2.x, constraint.p2.y, constraint.id);
}

void Triangulation::insertConstraint(T x1, T y1, T x2, T y2, unsigned int id)
{
	Vertex* v1 = findOrInsertVertex(x1, y1);
	Vertex* v2 = findOrInsertVertex(x2, y2);
	// Create the constraint and add it to the vector
	Constraint* c = new Constraint(id, x1, y1, x2, y2);
	v_constraint.push_back(c);
	c->vecIndex = v_constraint.size() -1;
	c->setEndVertices(v1,v2);

	//	std::cout<<"inserting constraint "<<id <<" (" << x1 <<" "<< y1<< " "<< x2 <<" "<< y2<<")" <<std::endl;
	insertLineConstraint(v1, v2, c);

	RENDER;
}

void Triangulation::insertLineConstraint(Vertex* v1, Vertex* v2, Constraint* c)
{
	//if the insertion is connected, just mark the unconstrained edge as constrained

	SymEdge* connectedEdge;
	if (isConnected(v1, v2, &connectedEdge)) {
		addConstraint(connectedEdge,c,v1,v2);
		return;
	}

	//checks if the insertion constains an existing edge
	//if so mark the existing edge constrained
	//and insert the remaining part using insertLineConstraint
	SymEdge* coincidentEdge;

	if (hasCoincidentSegments(v1,v2,&coincidentEdge)) {

		T x1, y1, x2, y2, x3, y3, x4, y4;
		x1 = v1->x;
		y1 = v1->y;
		x2 = v2->x;
		y2 = v2->y;

		x3 = coincidentEdge->vtx()->x;
		y3 = coincidentEdge->vtx()->y;

		x4 = coincidentEdge->dst()->x;
		y4 = coincidentEdge->dst()->y;

		if (onSegment(x1,y1,x2,y2,x4,y4))
		{
			addConstraint(coincidentEdge,c,v1,v2);
			insertLineConstraint(coincidentEdge->dst(),v2,c);
			return;
		}

		if (onSegment(x3,y3,x4,y4,x2,y2))
		{
			Vertex* theIntersect = insertPointOnEdge(coincidentEdge,x2,y2);
			SymEdge* e1 = theIntersect->outEdge;
			SymEdge* e2 = e1;

			do {
				Vertex* v4 = e1->dst();

				if (v4 == coincidentEdge->vtx())
				{
					addConstraint(e1,c,v1,v2);
					break;

				}

				e1 = e1->sym()->nxt();

			} while (e1 != e2);
		}

	}
	else
	{
		//the common case: insertion starts in a triangle and crosses at least one edge
		std::list<Edge*> l_ce;
		std::list<Vertex*> l_pi;

		// XXX: Find Constrained crossEdges isn't working
		findCrossEdgesLinear(v1, v2, l_ce);
		static int n_pass = 0;
		n_pass++;

		for(std::list<Edge*>::iterator it = l_ce.begin();
				it != l_ce.end();
				++it)
		{
			T vx, vy;

			bool intersect = findIntersect(v1->x, v1->y, v2->x, v2->y,
					(*it)->v1->x,
					(*it)->v1->y,
					(*it)->v2->x,
					(*it)->v2->y,
					vx,vy);

			if (intersect)
			{
				Vertex* theIntersection = insertPointOnEdge((*it)->outEdge, vx, vy);
				l_pi.push_back(theIntersection);
			}
			else
			{
				l_pi.push_back(NULL);
			}
		}

		Vertex* vStart = v1;
		l_pi.push_back(v2);

		//insert constrained intervals
		for(std::list<Vertex*>::iterator itrPi = l_pi.begin();
				itrPi != l_pi.end();
				++itrPi)
		{
			if (*itrPi == NULL)
			{
				continue;
			}

			insertLineConstraintSimple(vStart, *itrPi, c, v1, v2);
			vStart = *itrPi;
		}
	}//end else
}

void Triangulation::insertLineConstraintSimple(Vertex* v1, Vertex* v2, Constraint* c, Vertex* left, Vertex* right)
{
	//if the insertion is connected, just mark the unconstrained edge as constrained
	SymEdge* connectedEdge;
	if( isConnected(v1, v2, &connectedEdge))
	{
		addConstraint(connectedEdge,c,left,right);
		return;
	}

	SymEdge* coincidentEdge;
	if(hasCoincidentSegments(v1,v2,&coincidentEdge))
	{
		T x1, y1, x2, y2, x3, y3, x4, y4;
		x1 = v1->x;
		y1 = v1->y;
		x2 = v2->x;
		y2 = v2->y;

		x3 = coincidentEdge->vtx()->x;
		y3 = coincidentEdge->vtx()->y;

		x4 = coincidentEdge->dst()->x;
		y4 = coincidentEdge->dst()->y;
		if (onSegment(x1,y1,x2,y2,x4,y4))
		{
			addConstraint(coincidentEdge,c,left,right);

			insertLineConstraint(coincidentEdge->dst(),v2,c);
			return;
		}

		if (onSegment(x3,y3,x4,y4,x2,y2))
		{
			Vertex* target = coincidentEdge->vtx();

			if (!coincidentEdge->isConstrained)
			{
				Vertex* theIntersect = insertPointOnEdge(coincidentEdge,x2,y2,left,right);

				SymEdge* e1 = theIntersect->outEdge;
				SymEdge* e2 = e1;

				do
				{
					Vertex* v4 = e1->dst();

					if(v4 == target)
					{
						addConstraint(e1,c,left,right);
						break;

					}

					e1 = e1->sym()->nxt();
				} while (e1 != e2);

			}
			else
			{
				// XXX: result not needed? Vertex* theIntersect =
				insertPointOnEdge(coincidentEdge,x2,y2,left,right);
			}
		}
	}
	else //the common case: insertion starts in a triangle and crosses at least one unconstrained edge
	{
		/*
		std::list<SymEdge*> ul;
		//upper vertices
		std::list<Vertex*> pu;
		//lower vertices
		std::list<Vertex*> pl;

		findUnconstrainedCrossEdgesLinear(v1, v2,pu,pl,ul,left,right);

		Face* newFace = 0;
		//process the edges
		for (std::list<SymEdge*>::iterator e = ul.begin(); e != ul.end(); ++e)
		{
			Face* f1 = (*e)->fac();
			Face* f2 = (*e)->sym()->fac();
			newFace = joinFaces(f1, f2, (*e) );
		}

		SymEdge* constrainedEdge = splitFace(newFace,v1,v2);

		addConstraint(constrainedEdge,c,left,right);

		triangulatePseudoPolygon(pu,constrainedEdge);
		triangulatePseudoPolygon(pl,constrainedEdge->sym());
		*/
	}//end else
}

Face* Triangulation::locatePointInFace(T& x,T& y, Face* faceStart)
{
	Face* resultFace = NULL;

	if(faceStart == NULL) // If there is no face to start search from, do a linear (SLOW) search
	{
		// Skips face 0, because the point cant be outside limits.
		for (std::vector<Face*>::iterator itr = ++(v_face.begin());
				itr != v_face.end();
				++itr)
		{
			if (isPointInTri(x,y, *itr))
			{
				resultFace = (*itr);
				return resultFace;
			}
		}
	}
	else // We have a face to start the search.
	{
		// Search inside starting face first, for performance
		if (isPointInTri(x,y, faceStart))
		{
			resultFace = faceStart;
			return resultFace;
		}

		// Do an expansive search
		std::list<Face*> l_face;
		faceStart->ResetMark();
		faceStart->Mark();
		for(int i = 0; i < 3; ++i)
		{
			l_face.push_back(faceStart->getEdge(i)->sym()->fac());
		}

		while(!l_face.empty())
		{
			l_face.front()->Mark();

			if (isPointInTri(x,y, l_face.front()))
			{
				resultFace = l_face.front();
				return resultFace;
			}

			for(int i = 0; i < 3; ++i)
			{
				if (l_face.front()->getEdge(i)->sym()->fac()->IsMarked())
					continue;
				else // adjacent face is not marked
					l_face.push_back(l_face.front()->getEdge(i)->sym()->fac());
			}

			l_face.pop_front();
		}
	}

	if (resultFace == NULL)
		printf("\n WARNING: Point not found in triangulation.\n");

	return resultFace;
}

// Locate Methods
Triangulation::P_LOCAL Triangulation::locatePointLinear(
		T& x,
		T& y,
		Face** resultFace,
		SymEdge** resultEdge,
		Vertex** resultVertex)
{
	P_LOCAL res = P_LOCAL_NOTFOUND;

	*resultFace = locatePointInFace(x, y);

	if (resultFace == NULL)
	{
		printf("\n ERROR: Point not found in triangulation, inf function locatePointLinear\n Closing Triangulator...\n");
		fflush(stdout);
		exit(1);
	}
	else
		res = P_LOCAL_TRIANGLE;

	//extra test for point on vertex/edge
	if(onVertex(*resultFace , x, y, resultVertex))
	{
		res = P_LOCAL_VERTEX;
	}
	else if(onEdge(*resultFace , x, y, resultEdge))
	{
		res = P_LOCAL_EDGE;
	}

	return res;
}

Triangulation::P_LOCAL Triangulation::locatePoint(
		T& x,
		T& y,
		Face** resultFace,
		SymEdge** resultEdge,
		Vertex** resultVertex)
{
	P_LOCAL res = P_LOCAL_NOTFOUND;
	int n = v_vertex.size();

	int sampleSize =(int)ceil(pow(n, 1.0/3.0));
	T d = 10000.0;
	int resIndex = 1;

	for(int i= 0; i< sampleSize; i++)
	{
		int index = rand() % n;
		Vertex* tmp = v_vertex[index];
		T x2 = tmp->x;
		T y2 = tmp->x;

		T dx, dy;
		dx = x-x2;
		dy = y-y2;
		T distbetween = pow(dx*dx + dy*dy,1.0/2.0);
		if (distbetween < d)
		{
			d =distbetween;
			resIndex = tmp->vecIndex;
		}
	}

	int visited =0;
	res = locatePoint(v_vertex[resIndex]->outEdge->fac(),
			NULL,
			x, y,
			resultFace, resultEdge, resultVertex,
			visited);
	return res;
}

Triangulation::P_LOCAL Triangulation::locatePoint(
		Face* iniface,
		SymEdge* dummy,
		T& x, T& y,
		Face** resultFace,
		SymEdge** resultEdge,
		Vertex** resultVertex,
		int& visited)
{
	P_LOCAL res = P_LOCAL_NOTFOUND;
	Vertex* v1;
	Vertex* v2;
	SymEdge* e1;

	//add randomness to avoid loops in non-Delaunay triangulations
	double randNum = double(rand()) / double(0x7FFF);
	if (randNum > 0.5)
		e1 = iniface->getEdge(0);
	else
		e1 = iniface->getEdge(1);

	SymEdge* e2 = e1;

	// loop clockwise over the edges
	// XXX  Nao vejo logica nessa construcao. Ela funciona, porem o do/while parece nao fazer sentido
	do
	{
//		// teste
//		printf("\n e2 == %d  |  dummy == %d\n", (int)e2, (int) dummy);
//		fflush(stdout);

		if (e2 != dummy)
		{
			//skip one ccw test. The oppEdge of edge dummy is tested in the previous triangle.

			v1 = e2->vtx();
			v2 = e2->dst();

			// XXX teste
			int result = ccw(v2->x,v2->y,v1->x,v1->y,x,y);
			if (result == 0)
				printf ("\n Warning: CCW returned 0.");

			if (result < 0)
			{
				//check for enclosure

				dummy = e2->sym();

				//search on adjacent triangle
				return locatePoint(dynamic_cast<Face*>(dummy->fac()), // XXX: not nice ...
						dummy, x, y, resultFace, resultEdge, resultVertex, ++visited);
			}
		}

		e2 = e2->pri();

	} while (e2 != e1);


	//base case found: Triangle found with triangle vertices enclosing (x,y)
	//TRAP! It could also be on the edge.
	res = P_LOCAL_TRIANGLE;
	*resultFace = iniface;

	//extra test for point on vertex/edge
	if(onVertex(*resultFace , x, y, resultVertex))
	{
		res = P_LOCAL_VERTEX;
	}
	else if(onEdge(*resultFace , x, y, resultEdge))
	{
		res = P_LOCAL_EDGE;
	}

	//std::cout<<visited<<std::endl;
	return res;
}

Face* Triangulation::locateStartTriangle(Vertex* v1, Vertex* v2)
{
	SymEdge* e1 = v1->outEdge->nxt();
	SymEdge* e2 = e1;

	do
	{
		Vertex* v3 = e1->vtx();
		Vertex* v4 = e1->dst();

		if (segments_intersect(v1->x,v1->y,v2->x,v2->y,v3->x,v3->y,v4->x,v4->y))
			return e1->fac();

		e1 = e1->nxt()->sym()->nxt();

	} while(e1 != e2);

	//collinear case? should be handled by trivial case prior to common insert!
	SymEdge* e3 = v1->outEdge;

	do
	{
		if (onSegment(v1->x, v1->y,	v2->x, v2->y, e3->dst()->x, e3->dst()->y))
		{
			return e3->fac();
		}

		e3 = e3->pri()->sym();

	} while (e1 != e2);

	printf("Start face not found, Stop!");
	exit(-1);
}

bool Triangulation::onVertex(Face* f, T& x, T& y, Vertex** resultVertex)
{
	SymEdge* e = f->getEdge(0);
	SymEdge* en = e->nxt();
	SymEdge* enn = en->nxt();

	Vertex* v1=e->vtx();
	Vertex* v2=en->vtx();
	Vertex* v3=enn->vtx();

	if (overlap(x,y,v1->x,v1->y))
	{
		*resultVertex = v1;
		return true;
	}
	if (overlap(x,y,v2->x,v2->y))
	{
		*resultVertex = v2;
		return true;
	}
	if (overlap(x,y,v3->x,v3->y))
	{
		*resultVertex = v3;
		return true;
	}

	return false;
}

bool Triangulation::onEdge(Face* f, T& x, T& y, SymEdge** resultEdge)
{
	SymEdge* e = f->getEdge(0);
	SymEdge* en = e->nxt();
	SymEdge* enn = en->nxt();

	Vertex* v1=e->vtx();
	Vertex* v2=en->vtx();
	Vertex* v3=enn->vtx();

	if (onSegment(v1->x,v1->y,v2->x,v2->y,x,y))
	{
		*resultEdge = e;
		return true;
	}
	if (onSegment(v2->x,v2->y,v3->x,v3->y,x,y))
	{
		*resultEdge = en;
		return true;
	}
	if (onSegment(v3->x,v3->y,v1->x,v1->y,x,y))
	{
		*resultEdge = enn;
		return true;
	}

	return false;
}

/**
 * @return 0 if d lies on the circle spanned by a b c,
 *  +1 if d is inside,
 *  -1 if outside assuming ccw ordering of a b c.
 */
int Triangulation::inCircle(T& p1x, T& p1y,
		T& p2x, T& p2y,
		T& p3x, T& p3y,
		T& px,  T& py)
{
	// XXX N�o sei se essa conta est� certa...
	T adx = p1x - px;
	T bdx = p2x - px;
	T cdx = p3x - px;
	T ady = p1y - py;
	T bdy = p2y - py;
	T cdy = p3y - py;

	T bdxcdy = bdx * cdy;
	T cdxbdy = cdx * bdy;
	T alift = adx * adx + ady * ady;

	T cdxady = cdx * ady;
	T adxcdy = adx * cdy;
	T blift  = bdx * bdx + bdy * bdy;

	T adxbdy = adx * bdy;
	T bdxady = bdx * ady;
	T clift  = cdx * cdx + cdy * cdy;

	T det =   alift * (bdxcdy - cdxbdy)
			+ blift * (cdxady - adxcdy)
			+ clift * (adxbdy - bdxady);

	// XXX usar Epsilon?
	if (det > 0.) return +1;
	if (det < 0.) return -1;
//	printf("\ninCircle: 0 value Found:\n( %3.0f , %3.0f)\n( %3.0f , %3.0f)\n( %3.0f , %3.0f)\n( %3.0f , %3.0f)\n",
//			p1x, p1y, p2x, p2y, p3x, p3y, px, py);
//	if (p1x * p1y * p2x * p2y * p3x * p3y * px * py < 0.0)
//		printf("\n\n\n --- PALA DO MAU! --- \n\n\n");
	return 0;
}

// a pseudo segment intersect check based on ccw checks
bool Triangulation::segments_intersect(T& x1, T& y1,
		T& x2, T& y2,
		T& x3, T& y3,
		T& x4, T& y4)
{
	int a = ccw(x1,y1,x2,y2,x3,y3);
	int b = ccw(x1,y1,x2,y2,x4,y4);

	if ((a < 0 && b < 0) || (a > 0 && b > 0))
	{
		return false;
	}
	else
	{
		int c = ccw(x3,y3,x4,y4,x1,y1);
		int d = ccw(x3,y3,x4,y4,x2,y2);

		if ((c < 0 && d < 0) || (c > 0 && d > 0))
			return false;
	}

	return true;
}

bool Triangulation::findIntersect(T& x1, T& y1,
		T& x2, T& y2,
		T& x3, T& y3,
		T& x4, T& y4,
		T& vx, T& vy)
{
	T denom = (y4-y3)*(x1-x2)-(y1-y2)*(x4-x3);
	if (denom == 0) // There is no intersection when denom == 0
		return false;

	T nume_a = ((y4-y3)*(x4-x2)-(x4-x3)*(y4-y2));
	vx = (nume_a/denom)*(x1-x2) + x2;
	vy = (nume_a/denom)*(y1-y2) + y2;

	// Check if the x and y coordinates are within both lines
	if (!(insideArea(x1, y1, x2, y2, vx, vy)))
		return false;
	if (!(insideArea(x3, y3, x4, y4, vx, vy)))
		return false;

	return true;
}

bool Triangulation::findIntersectParam(T x1, T y1,
		T x2, T y2,
		T x3, T y3,
		T x4, T y4,
		T &vx, T &vy)
{
	T denom = (y4-y3)*(x1-x2)-(y1-y2)*(x4-x3);
	if (denom == 0) // There is no intersection when denom == 0
		return false;

	T nume_a = ((y4-y3)*(x4-x2)-(x4-x3)*(y4-y2));
	vx = (nume_a/denom)*(x1-x2) + x2;
	vy = (nume_a/denom)*(y1-y2) + y2;

	// Check if the x and y coordinates are within both lines
	if (!(insideArea(x1, y1, x2, y2, vx, vy)))
		return false;
	if (!(insideArea(x3, y3, x4, y4, vx, vy)))
		return false;

	return true;
}

bool Triangulation::overlap(T& x1, T& y1, T& x2, T& y2)
{
	return ((x1 == x2) && (y1 == y2));
}


bool Triangulation::onSegment(T& x1, T& y1, T& x2, T& y2, T& x, T& y)
{
	bool collinear = (ccw(x1, y1, x2, y2, x, y) == 0);

	if(!collinear)
		return false;
	else if (insideArea(x1, y1, x2, y2, x, y))
		return true;
	return false;
}

//position test. is (x,y) between (x1,y1) and (x2,y2)?
bool Triangulation::insideArea(T& x1, T& y1, T& x2, T& y2, T& x, T& y)
{
	T xMax = x1 > x2 ?  x1 : x2;
	T xMin = x1 < x2 ?  x1 : x2;
	T yMax = y1 > y2 ?  y1 : y2;
	T yMin = y1 < y2 ?  y1 : y2;

	if( x<xMin || x>xMax)
		return false;
	if( y<yMin || y>yMax)
		return false;

	return true;
}


bool Triangulation::hasCoincidentSegments(Vertex* v1, Vertex* v2, SymEdge** coincidentEdge)
{
	SymEdge* e1 = v1->outEdge;
	SymEdge* e2 = e1;

	do
	{
		Vertex* v3 = e1->dst();

		if(onSegment(v1->x,v1->y,v2->x,v2->y,v3->x,v3->y) || onSegment(v1->x,v1->y,v3->x,v3->y,v2->x,v2->y)) {
			*coincidentEdge = e1;
			return true;
		}

		e1 = e1->sym()->nxt();

	} while (e1 != e2);

	return false;
}


void Triangulation::findConstrainedCrossEdges(Vertex* v1, Vertex* v2,
		std::list<std::pair<int,SymEdge*> >& cl,
		std::vector<Vertex*>& pm)
{
	int vertexIndex = 0;
	Face* t = locateStartTriangle(v1, v2);
	Vertex* v = v1;
	SymEdge* shareEdge;
	getOpposedFace(t, v,&shareEdge);

	if (shareEdge->isConstrained)
	{
		cl.push_back(std::pair<int,SymEdge* >(vertexIndex,shareEdge));
		pm.push_back(NULL);
		vertexIndex++;
	}

	Vertex* vSeg = shareEdge->sym()->nxt()->dst();

	while (vSeg != v2)
	{
		if (vertexAboveSegment(v1,v2,vSeg))
		{
			shareEdge = shareEdge->sym()->nxt();
		}
		else if(vertexBelowSegment(v1,v2,vSeg))
		{
			shareEdge = shareEdge->sym()->nxt()->nxt();
		}
		else
		{
			shareEdge = shareEdge->sym()->nxt()->nxt();
			pm.push_back(vSeg);
			vertexIndex++;
		}

		if (shareEdge->isConstrained)
		{
			cl.push_back(std::pair<int,SymEdge* >(vertexIndex,shareEdge));
			pm.push_back(NULL);
			vertexIndex++;
		}

		vSeg = shareEdge->sym()->nxt()->dst();
	}//end while
}

void Triangulation::findConstrainedCrossEdgesLinear(Vertex* v1, Vertex* v2,
		std::list<std::pair<int,SymEdge*> >& cl,
		std::vector<Vertex*>& pm)
{
	int vertexIndex = 0;
	for (std::vector<SymEdge*>::iterator itr_symEdge = v_symEdge.begin();
			itr_symEdge != v_symEdge.end();
			++itr_symEdge)
	{
		if ((*itr_symEdge)->isConstrained)
		{
			T vx, vy;

			bool intersect = findIntersect(v1->x, v1->y, v2->x, v2->y,
					(*itr_symEdge)->edg()->v1->x,
					(*itr_symEdge)->edg()->v1->y,
					(*itr_symEdge)->edg()->v2->x,
					(*itr_symEdge)->edg()->v2->y,
					vx,vy);

			if (intersect)
			{
				cl.push_back(std::pair<int,SymEdge* >(vertexIndex++,(*itr_symEdge)));
				pm.push_back(NULL);
			}
		}
	}
}

void Triangulation::findUnconstrainedCrossEdges(Vertex* v1, Vertex* v2,
		std::list<Vertex*>& pu,
		std::list<Vertex*>& pl,
		std::list<SymEdge*>& ul,
		Vertex* /*left*/, Vertex* right)
{
	Face* t = locateStartTriangle(v1, right);
	Vertex* v = v1;
	SymEdge* shareEdge;

	getOpposedFace(t, v,&shareEdge);

	if(!shareEdge->isConstrained)
	{
		ul.push_back(shareEdge);
	}

	//initialize the vertex list pu and pl
	if(vertexAboveSegment(v1, v2,shareEdge->vtx()))
	{
		pu.push_back(shareEdge->vtx());
		pl.push_front(shareEdge->dst());
	}
	else
	{
		pu.push_back(shareEdge->dst());
		pl.push_front(shareEdge->vtx());
	}

	Vertex* vSeg = shareEdge->sym()->nxt()->dst();

	while(vSeg != v2)
	{
		if(vertexAboveSegment(v1,v2,vSeg))
		{
			pu.push_back(vSeg);
			shareEdge = shareEdge->sym()->nxt();
		}
		else
		{
			pl.push_front(vSeg);
			shareEdge = shareEdge->sym()->pri();
		}

		if(!shareEdge->isConstrained)
		{
			ul.push_back(shareEdge);
		}
		vSeg = shareEdge->sym()->nxt()->vtx();

	}//end while
}

void Triangulation::findUnconstrainedCrossEdgesLinear(Vertex* v1, Vertex* v2,
		std::list<Vertex*>& pu,
		std::list<Vertex*>& pl,
		std::list<SymEdge*>& ul,
		Vertex* /*left*/, Vertex* right)
{
	//int vertexIndex = 0;

	for (std::vector<SymEdge*>::iterator itr_symEdge = v_symEdge.begin();
			itr_symEdge != v_symEdge.end();
			++itr_symEdge)
	{
		T vx, vy;

		bool intersect = findIntersect(v1->x, v1->y, v2->x, v2->y,
				(*itr_symEdge)->edg()->v1->x,
				(*itr_symEdge)->edg()->v1->y,
				(*itr_symEdge)->edg()->v2->x,
				(*itr_symEdge)->edg()->v2->y,
				vx,vy);

		if (intersect)
		{
			ul.push_back(*itr_symEdge);
			//cl.push_back(std::pair<int,SymEdge* >(vertexIndex++,(*itr_symEdge)));
		}
		else // Check if the triangle intersects, and adds to lower and upper lists
		{
			//if((*itr_symEdge)->fac() )
		}
	}
}

void Triangulation::findCrossEdgesLinear(Vertex* v1, Vertex* v2, std::list<Edge*> &l_edge)
{
	for (std::vector<Edge*>::iterator itr_edge = v_edge.begin();
			itr_edge != v_edge.end();
			++itr_edge)
	{
		T vx, vy;

		bool intersect = findIntersect(v1->x, v1->y, v2->x, v2->y,
				(*itr_edge)->v1->x,
				(*itr_edge)->v1->y,
				(*itr_edge)->v2->x,
				(*itr_edge)->v2->y,
				vx,vy);

		if (intersect)
		{
			l_edge.push_back(*itr_edge);
		}
	}
}

/** @return 0 if p1, p2 and p3 are collinear,
 * +1 if they are CW,
 * -1 if they are CCW.
 */
int Triangulation::ccw(T x1, T y1, T x2, T y2, T x3, T y3)
{
	T det = (x1*y2 + x2*y3 + x3*y1) - (x2*y1 + x3*y2 + x1*y3);
	//T det = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
	//T det = (x1 - x3) * (y2 - y3) - (x2 - x3) * (y1 - y3);

	// XXX Epsilon?
	if (det > 0.0)
		return +1; // ClockWise
	if (det < 0.0)
		return -1; // CounterClockwise
	return 0; // Collinear
}

int Triangulation::ccw(Vertex* v_a, Vertex* v_b, Vertex* v_c)
{
	return (ccw(v_a->x,v_a->y,v_b->x,v_b->y,v_c->x,v_c->y));
}

int Triangulation::ccw(Point<float> p_a, Point<float> p_b, Point<float> p_c)
{
	return (ccw(p_a.x,p_a.y,p_b.x,p_b.y,p_c.x,p_c.y));
}

Triangulation::T Triangulation::sign(T x1, T y1, T x2, T y2, T x3, T y3)
{
	return (x1 - x3) * (y2 - y3) - (x2 - x3) * (y1 - y3);
}

bool Triangulation::isPointInTri(T x, T y, Face* face)
{
	/*
	if (sign(x, y, face->getEdge(0)->vtx()->x, face->getEdge(0)->vtx()->y, face->getEdge(1)->vtx()->x, face->getEdge(1)->vtx()->y) < 0.0)
		return false;
	if (sign(x, y, face->getEdge(1)->vtx()->x, face->getEdge(1)->vtx()->y, face->getEdge(2)->vtx()->x, face->getEdge(2)->vtx()->y) < 0.0)
		return false;
	if (sign(x, y, face->getEdge(2)->vtx()->x, face->getEdge(2)->vtx()->y, face->getEdge(0)->vtx()->x, face->getEdge(0)->vtx()->y) < 0.0)
		return false;
	 */

	if (ccw(x, y, face->getEdge(0)->vtx()->x, face->getEdge(0)->vtx()->y, face->getEdge(0)->nxt()->vtx()->x, face->getEdge(0)->nxt()->vtx()->y) > 0.0)
		return false;
	if (ccw(x, y, face->getEdge(1)->vtx()->x, face->getEdge(1)->vtx()->y, face->getEdge(1)->nxt()->vtx()->x, face->getEdge(1)->nxt()->vtx()->y) > 0.0)
		return false;
	if (ccw(x, y, face->getEdge(2)->vtx()->x, face->getEdge(2)->vtx()->y, face->getEdge(2)->nxt()->vtx()->x, face->getEdge(2)->nxt()->vtx()->y) > 0.0)
		return false;

	return true;
}

Vertex* Triangulation::findOrInsertVertex(T& x, T& y)
{
	Face* resultFace;
	SymEdge* resultEdge;
	Vertex* resultVertex;
	P_LOCAL resType;

	// XXX This point search is very slow... But I can guarantee it works
	resType = locatePointLinear(x, y, &resultFace,&resultEdge,&resultVertex);

	switch (resType)
	{
		case P_LOCAL_VERTEX:
			return resultVertex;
			break;
		case P_LOCAL_EDGE:
			return insertPointOnEdge(resultEdge, x, y);//XXX , resultEdge->getLeftEnd().second, resultEdge->getRightEnd().second );
			break;
		case P_LOCAL_TRIANGLE:
			return insertPointInFace(resultFace, x, y);
			break;
		case P_LOCAL_NOTFOUND:
		default:
			printf ( "Error: Not in domain, or faces not ccw!\n" );
			exit(-1);
			return nullptr;
			break;
	}
	return nullptr;
}

Vertex* Triangulation::insertPointInFace(Face* face,T& x, T& y)
{
	SymEdge* e1 = face->getEdge(0);
	SymEdge* e2 = e1->nxt();
	SymEdge* e3 = e2->nxt();

	Vertex* v1 = e1->vtx();
	Vertex* v2 = e2->vtx();
	Vertex* v3 = e3->vtx();

	Vertex* v5 = createVertex(x,y);

	//face 1
	SymEdge* e7 = createSymEdge(v5, v1);
	SymEdge* e9 = createSymEdge(v2, v5);
	createFace(e7, e9, e1);

	//face 2
	SymEdge* e11 = e9->sym();
	SymEdge* e13 = createSymEdge(v3, v5);

	createFace(e11, e2,e13);

	//face 3
	createFace(e13->sym(), e3, e7->sym());

	//remove face
	removeFace(face);

	//restore delaunay property
	legalizedEdge(e1);
	legalizedEdge(e2);
	legalizedEdge(e3);

	return v5;
}

Vertex* Triangulation::insertPointOnEdge(SymEdge* e, T& x, T& y)
{
	SymEdge* e1 = e;
	SymEdge* e2 = e1->nxt();
	SymEdge* e3 = e2->nxt();

	Vertex* v1 = e1->vtx();
	Vertex* v2 = e2->vtx();

	if ((x == v1->x) && (y == v1->y))
		return v1;

	if ((x == v2->x) && (y == v2->y))
		return v2;

	Vertex* v5 = createVertex(x,y);

	splitAndLinkEdge(e1 ,v5);

	SymEdge* e5 = e3->nxt()->sym()->nxt();
	SymEdge* e6 = e2->pri()->sym()->pri();

	legalizedEdge(e2);
	legalizedEdge(e3);
	legalizedEdge(e5);
	legalizedEdge(e6);

#ifdef PF2D_RENDER
	//showConstraints();
#endif //PF2D_RENDER

	return v5;
}

Vertex* Triangulation::insertPointOnEdge(SymEdge* e, T& x, T& y,Vertex* left, Vertex* right)
{
	SymEdge* e1 = e;
	SymEdge* e2 = e1->nxt();
	SymEdge* e3 = e2->nxt();

	Vertex* v1 = e1->vtx();
	Vertex* v2 = e2->vtx();

	if (x == v1->x && y == v1->y)
		return v1;

	if (x == v2->x && y == v2->y)
		return v2;

	Vertex* v5 = createVertex(x,y);

	splitAndLinkEdge(e1 ,v5,left,right);

	SymEdge* e5 = e3->nxt()->sym()->nxt();
	SymEdge* e6 = e2->pri()->sym()->pri();

	legalizedEdge(e2);
	legalizedEdge(e3);
	legalizedEdge(e5);
	legalizedEdge(e6);

	return v5;
}

void Triangulation::legalizedEdge(SymEdge* e)
{
	if (e->isConstrained ||e->isBoundary)
	{
		return;
	}
	else
	{
		if (!isLegalEdge(e))
		{
			SymEdge* ei = e->sym()->nxt();
			SymEdge* ek = ei->nxt();

			flip(e);

			legalizedEdge(ei);
			legalizedEdge(ek);
		}
	}
}

bool Triangulation::isLegalEdge(SymEdge* e)
{
	if ((e->isConstrained) || (e->isBoundary))
		return true;

	Vertex* v1 = e->vtx();
	Vertex* v2 = e->dst();

	Vertex* v3 = e->nxt()->dst();
	Vertex* v4 = e->sym()->nxt()->dst();

	return (inCircle(v1->x, v1->y,
			v2->x, v2->y,
			v3->x, v3->y,
			v4->x, v4->y) > 0);
}

bool Triangulation::isConnected(Vertex* v1Itr, Vertex* v2Itr)
{
	SymEdge* e1 = v1Itr->outEdge;
	SymEdge* e2 = e1;

	do {
		Vertex* v3Itr = e1->nxt()->vtx();
		if(v3Itr == v2Itr) {
			return true;
		}

		e1 = e1->sym()->nxt();
	} while (e1 != e2);

	return false;
}

bool Triangulation::isConnected(Vertex* v1Itr, Vertex* v2Itr, SymEdge** eItr)
{
	SymEdge* e1 = v1Itr->outEdge;
	SymEdge* e2 = e1;

	do
	{
		Vertex* v3Itr = e1->nxt()->vtx();
		if(v3Itr == v2Itr)
		{
			*eItr = e1;
			return true;
		}

		e1 = e1->sym()->nxt();
	} while(e1 != e2);

	return false;
}

//new implementation of flip that reuses existing faces.

SymEdge* Triangulation::flip(SymEdge* e)
{
	//the ccw edges
	SymEdge* eOpp = e->sym();
	SymEdge* e2 = e->nxt();
	SymEdge* e3 = e2->nxt();
	SymEdge* e4 = eOpp->nxt();
	SymEdge* e5 = e4->nxt();

	//create the new edge
	Vertex* v3 = e2->dst();
	Vertex* v4 = e4->dst();

	//old faces
	Face* f1 = e->fac();
	Face* f2 = eOpp->fac();

	//SymEdge* e6 = createSymEdge(v3,v4);
	//SymEdge* e7 = e6->sym();
	//removeSymEdge(e);
	resetSymEdge(e,v3,v4);

	//f1->setEdges(e6, e5, e2);
	//f2->setEdges(e7, e3, e4);
	f1->setEdges(e, e5, e2);
	f2->setEdges(eOpp, e3, e4);

	//e = e6;
	return e;
}

void Triangulation::triangulatePseudoPolygon(std::list<Vertex*>& l_vector, SymEdge* e)
{
	if (l_vector.size() > 1)
	{
		Vertex* a = e->vtx();
		Vertex* b = e->dst();
		Vertex* c = *(l_vector.begin());
		std::list<Vertex*>::iterator cit = l_vector.begin();

		for (std::list<Vertex*>::iterator itr = l_vector.begin();
				itr != l_vector.end();
				++itr)
		{
			if (inCircle(a->x, a->y,b->x, b->y,c->x, c->y,(*itr)->x, (*itr)->y))
			{
				c = *itr;
				cit = itr;
			}
		}

		std::list<Vertex*> pe;
		pe.insert(pe.begin(), l_vector.begin(), cit);

		std::list<Vertex*> pd;
		pd.insert(pd.begin(), ++cit, l_vector.end());

		SymEdge* peEdge = 0;
		SymEdge* pdEdge = 0;

		if (e->nxt()->dst() == c)
		{
			peEdge = splitFace(e->fac(), a, c);

			//switch to the outer SimplePolygon face edge
			if(peEdge->fac() == e->fac())
				peEdge = peEdge->sym();

		}
		else if (e->pri()->vtx() == c)
		{
			pdEdge = splitFace(e->fac(), b, c);

			//switch to the outer SimplePolygon face edge
			if (pdEdge->fac() == e->fac())
				pdEdge = pdEdge->sym();

		}
		else
		{
			peEdge = splitFace(e->fac(), a, c);

			//switch to the outer SimplePolygon face edge
			if (peEdge->fac() == e->fac())
				peEdge = peEdge->sym();

			pdEdge = splitFace(e->fac(), b, c);

			//switch to the outer SimplePolygon face edge
			if (pdEdge->fac() == e->fac())
				pdEdge = pdEdge->sym();
		}

		triangulatePseudoPolygon(pe,peEdge);
		triangulatePseudoPolygon(pd,pdEdge);
	}
}

// operators
Vertex* Triangulation::createVertex(T x, T y)
{
	Vertex* v = new Vertex(x,y);
	v_vertex.push_back(v);
	v->vecIndex = v_vertex.size() -1;
//	if(v->vecIndex >= 613)
//		printf("\nVERTEX: %d == ( %3.0f, %3.0f)\n\n",v->vecIndex,v->x, v->y);
	fflush(stdout);
	return v;
}

Edge* Triangulation::createEdge(Vertex* v1, Vertex* v2)
{
	Edge* e = new Edge(v1, v2);
	v_edge.push_back(e);
	e->vecIndex = v_edge.size() -1;
	return e;
}

SymEdge* Triangulation::createSymEdge(Vertex* v1, Vertex* v2)
{
	Edge* e = createEdge(v1,v2);

	//construct symetrical edges and connect them
	SymEdge* e1 = new SymEdge(v1, e);
	v_symEdge.push_back(e1);
	e1->vecIndex = v_symEdge.size() -1;

	SymEdge* e2 = new SymEdge(v2, e);
	v_symEdge.push_back(e2);
	e2->vecIndex = v_symEdge.size() -1;
	e->outEdge = e2;

	// Note: we need to find a new incident outedge with operators flip() and spliceFace() which trigger removeEdge();
	v1->outEdge = e1;
	v2->outEdge = e2;

	// Note: To correctly initialize the symedges, faces are are needed.
	e1->setNxt(NULL);
	e1->setRot(NULL);
	e1->setSym(e2);

	e2->setNxt(NULL);
	e2->setRot(NULL);
	e2->setSym(e1);

	return e1;
}

Face* Triangulation::createFace(SymEdge* e1, SymEdge* e2, SymEdge* e3)
{
	// SymEdge internal pointer structure is built inside the Face Constructor
	Face* f = new Face();
	f->setEdges(e1,e2,e3);

	v_face.push_back(f);
	f->vecIndex = v_face.size() -1;

	return f;
}

Face* Triangulation::createFace(std::vector<SymEdge*>& edges)
{
	// SymEdge internal pointer structure is built inside the Face Constructor
	Face* f = new Face();
	f->setEdges(edges);

	v_face.push_back(f);
	f->vecIndex = v_face.size() -1;

	for (std::vector<SymEdge*>::iterator itr = edges.begin();
			itr != edges.end();
			++itr)
	{
		(*itr)->setFac(f);
	}

	return f;
}

SymEdge* Triangulation::resetSymEdge(SymEdge* e, Vertex* v1, Vertex* v2)
{
	// Change the direction of the edge
	e->edg()->resetEdge(v1,v2);

	// Change the outEdge of the old vtx
	if (e->vtx()->outEdge == e)
		e->vtx()->outEdge = e->nxt()->nxt()->sym();
	if (e->dst()->outEdge == e->sym())
		e->dst()->outEdge = e->sym()->nxt()->nxt()->sym();

	// Chenge the symEdges
	e->resetSymEdge(v1,e->edg());
	e->sym()->resetSymEdge(v2,e->edg());

	// Note: we need to find a new incident outedge with operators flip() and spliceFace() which trigger removeEdge();
	v1->outEdge = e;
	v2->outEdge = e->sym();

	return e;
}

void Triangulation::removeFace(Face* f)
{
	v_face[f->vecIndex] = v_face.back();
	v_face[f->vecIndex]->vecIndex = f->vecIndex;
	v_face.pop_back();
	delete f;
}

void Triangulation::removeEdge(Edge* e)
{
	v_edge[e->vecIndex] = v_edge.back();
	v_edge[e->vecIndex]->vecIndex = e->vecIndex;
	v_edge.pop_back();
	delete e;
}

void Triangulation::removeSymEdge(SymEdge* e)
{
	//find a new out edge for org and dest of e
	e->vtx()->outEdge = e->sym()->nxt();
	e->nxt()->vtx()->outEdge = e->nxt();

	// Delete the edge
	removeEdge(e->edg());
	e->setEdg(NULL);
	e->sym()->setEdg(NULL);

	v_symEdge[e->sym()->vecIndex] = v_symEdge.back();
	v_symEdge[e->sym()->vecIndex]->vecIndex = e->sym()->vecIndex;
	v_symEdge.pop_back();
	delete e->sym();

	v_symEdge[e->vecIndex] = v_symEdge.back();
	v_symEdge[e->vecIndex]->vecIndex = e->vecIndex;
	v_symEdge.pop_back();
	delete e;
}

void Triangulation::removeVertex(Vertex* v)
{
	v_vertex[v->vecIndex] = v_vertex.back();
	v_vertex[v->vecIndex]->vecIndex = v->vecIndex;
	v_vertex.pop_back();
	delete v;
}

SymEdge* Triangulation::splitFace(Face* f, Vertex* v1, Vertex* v2)
{
	SymEdge* e_split = createSymEdge(v1,v2);
	SymEdge* itr_e1 = 0;
	SymEdge* itr_e2 = 0;

	// XXX TRETA:
	//locate the start edge originated from v1 and v2;
	for(int i = 0; i < f->getEdgeCount(); i++)
	{
		SymEdge* tmp = f->getEdge(i);

		if(tmp->vtx() == v1)
			itr_e1 = tmp;

		if(tmp->vtx() == v2)
			itr_e2 = tmp;
	}

	std::vector<SymEdge*> vec1;

	//store the edges in the half face
	SymEdge* itr_e3 = itr_e1;

	do
	{
		vec1.push_back(itr_e3);
		itr_e3->setFac(f);
		itr_e3 = itr_e3->nxt();
	} while (itr_e3 != itr_e2);

	vec1.push_back(e_split->sym());
	e_split->sym()->setFac(f);

	//store the edges in the other half face
	std::vector<SymEdge*> vec2;
	SymEdge* itr_e4 = itr_e2;
	do
	{
		vec2.push_back(itr_e4);
		itr_e4 = itr_e4->nxt();
	} while (itr_e4 != itr_e1);

	vec2.push_back(e_split);

	//reuse f

	f->setEdges(vec1);

	// XXX: this looks suspicious, don't we need to store the
	// face address somewhere?
	createFace(vec2);

	return e_split;
}

Face* Triangulation::joinFaces(Face* f1, Face* f2, SymEdge* e1Itr)
{
	if (f1 == f2)
	{
		std::vector<SymEdge*> edges;
		SymEdge* e2Itr = e1Itr;
		e2Itr = e2Itr->nxt();

		do
		{
			edges.push_back(e2Itr);
			e2Itr->setFac(f1);
			e2Itr = e2Itr->nxt();
		} while (e2Itr != e1Itr->sym());

		//reuse f1
		f1->setEdges(edges);
		removeSymEdge(e1Itr);

	}
	else
	{
		std::vector<SymEdge*> edges;
		SymEdge* e2Itr = e1Itr;
		e2Itr = e2Itr->nxt();

		do
		{
			edges.push_back(e2Itr);
			e2Itr->setFac(f1);
			e2Itr = e2Itr->nxt();

		} while (e2Itr != e1Itr);

		SymEdge* e1OppItr = e1Itr->sym();
		SymEdge* e4Itr = e1OppItr;
		e4Itr = e4Itr->nxt();

		do
		{
			edges.push_back(e4Itr);
			e4Itr->setFac(f1);
			e4Itr = e4Itr->nxt();

		} while(e4Itr != e1OppItr);

		//reuse f1
		f1->setEdges(edges);
		removeSymEdge(e1Itr);
		removeFace(f2);
	}

	return f1;
}

void Triangulation::splitAndLinkEdge(SymEdge* e1, Vertex* v5)
{
	SymEdge* e2 = e1->sym();
	SymEdge* e3 = e1->nxt();
	SymEdge* e4 = e3->nxt();

	SymEdge* e5 = e2->nxt();
	SymEdge* e6 = e5->nxt();

	Vertex* v1 = e1->vtx();

	Vertex* v3 = e4->vtx();
	Vertex* v4 = e6->vtx();

	Face* f1 = e1->fac();
	Face* f2 = e2->fac();

	//process face 1
	SymEdge* e7 = createSymEdge(v3,v5);
	SymEdge* e8 = e7->sym();

	// Set mid edge vertex to the new vertex
	if (e1->vtx() == e1->edg()->v1)
		e1->edg()->v1 = v5;
	else e1->edg()->v2 = v5;
	e1->setVtx(v5);

	SymEdge* e9 = createSymEdge(v1,v5);
	SymEdge* e10 = e9->sym();

	if (e1->isConstrained)
	{
		e9->setConstrained(e1->getConstraints());
		e10->setConstrained(e1->getConstraints());
	}

	std::vector<SymEdge*> edges1;
	edges1.push_back(e1);
	edges1.push_back(e3);
	edges1.push_back(e7);
	e1->setFac(f1);
	e3->setFac(f1);
	e7->setFac(f1);
	f1->setEdges(edges1);

	std::vector<SymEdge*> edges2;
	edges2.push_back(e9);
	edges2.push_back(e8);
	edges2.push_back(e4);
	createFace(edges2);

	//process face 2
	SymEdge* e11 = createSymEdge(v4,v5);
	SymEdge* e12 = e11->sym();

	std::vector<SymEdge*> edges3;
	edges3.push_back(e2);
	edges3.push_back(e12);
	edges3.push_back(e6);
	e2->setFac(f2);
	e12->setFac(f2);
	e6->setFac(f2);
	f2->setEdges(edges3);

	std::vector<SymEdge*> edges4;
	edges4.push_back(e10);
	edges4.push_back(e5);
	edges4.push_back(e11);
	createFace(edges4);
}

void Triangulation::splitAndLinkEdge(SymEdge* e1, Vertex* v5,Vertex* left,Vertex* right)
{
	SymEdge* e2 = e1->sym();
	SymEdge* e3 = e1->nxt();
	SymEdge* e4 = e3->nxt();

	SymEdge* e5 = e2->nxt();
	SymEdge* e6 = e5->nxt();

	Vertex* v1 = e1->vtx();
	// XXX: not needed? Vertex* v2 = e3->vtx();
	Vertex* v3 = e4->vtx();
	Vertex* v4 = e6->vtx();

	Face* f1 = e1->fac();
	Face* f2 = e2->fac();

	//process face 1
	SymEdge* e7 = createSymEdge(v3,v5);
	SymEdge* e8 = e7->sym();

	// Set mid edge vertex to the new vertex
	if (e1->vtx() == e1->edg()->v1)
		e1->edg()->v1 = v5;
	else e1->edg()->v2 = v5;
	e1->setVtx(v5);

	SymEdge* e9 = createSymEdge(v1,v5);
	SymEdge* e10 = e9->sym();

	if (e1->isConstrained)
	{
		e9->setConstrained(e1->getConstraints());
		e10->setConstrained(e1->getConstraints());

		e1->vtx()->isConstrained = true;
		e1->sym()->vtx()->isConstrained = true;
		e2->vtx()->isConstrained = true;
		e2->sym()->vtx()->isConstrained = true;
		e9->vtx()->isConstrained = true;
		e9->sym()->vtx()->isConstrained = true;
		e10->vtx()->isConstrained = true;
		e10->sym()->vtx()->isConstrained = true;
	}

	std::vector<SymEdge*> edges1;
	edges1.push_back(e1);
	edges1.push_back(e3);
	edges1.push_back(e7);
	e1->setFac(f1);
	e3->setFac(f1);
	e7->setFac(f1);
	f1->setEdges(edges1);

	std::vector<SymEdge*> edges2;
	edges2.push_back(e9);
	edges2.push_back(e8);
	edges2.push_back(e4);
	createFace(edges2);

	//process face 2
	SymEdge* e11 = createSymEdge(v4,v5);
	SymEdge* e12 = e11->sym();

	// XXX is tthis needed? e2->setDestVertex(v5);
	std::vector<SymEdge*> edges3;
	edges3.push_back(e2);
	edges3.push_back(e12);
	edges3.push_back(e6);
	e2->setFac(f2);
	e12->setFac(f2);
	e6->setFac(f2);
	f2->setEdges(edges3);

	std::vector<SymEdge*> edges4;
	edges4.push_back(e10);
	edges4.push_back(e5);
	edges4.push_back(e11);
	createFace(edges4);
}

void Triangulation::addConstraint(SymEdge* e, Constraint* c, Vertex* left, Vertex* right)
{
	left->isConstrained = true;
	right->isConstrained = true;
	e->edg()->isConstrained = true;

	e->setConstrained(c);
	e->sym()->setConstrained(c);

	c->addSymEdge(e);
	c->addSymEdge(e->sym());
}

Face* Triangulation::getOpposedFace(Face* f, Vertex* v, SymEdge** shareEdge)
{
	SymEdge* e1Itr = f->getEdge(0);
	SymEdge* e2Itr = e1Itr;

	do
	{
		Vertex* v1Itr = e1Itr->vtx();
		if (v1Itr == v)
			break;
		e1Itr = e1Itr->nxt();

	} while (e1Itr != e2Itr);

	*shareEdge = e1Itr->nxt();

	return (*shareEdge)->sym()->fac();
}

bool Triangulation::vertexAboveSegment(Vertex* v1, Vertex* v2, Vertex* v)
{
	return (ccw(v1->x, v1->y, v2->x, v2->y, v->x,  v->y) > 0);
}

//It is used to find constrained edges
bool Triangulation::vertexBelowSegment(Vertex* v1, Vertex* v2, Vertex* v)
{
	return (ccw(v1->x, v1->y, v2->x, v2->y, v->x,  v->y) < 0);
}

#ifdef _DEBUG
void Triangulation::printTriangulation()
{
	std::ofstream output;
	output.open("triangulationResult.txt", std::ios::trunc);

	output << "\n\n_________________________________________________________\n\n\n";

	output << "Triangulation Results:\n\n";
	output << "Size of the map (W,H): " << width << ", " << height;
	output << "\n\n";

	output << "Number of:\n";
	output << "Vertices: " << v_vertex.size() << "\n";
	output << "Edges:    " << v_edge.size() << "\n";
	output << "SymEdges: " << v_symEdge.size() << "\n";
	output << "Faces:    " << v_face.size() << "\n";
	output << "\n";

	output << "Number and Vertices of each Face, in order:\n";

	for (std::vector<Face*>::iterator itr_face = v_face.begin();
			itr_face != v_face.end();
			++itr_face)
	{
		output << (*itr_face)->vecIndex << " Face: ";

		for (int i = 0; i< (*itr_face)->getEdgeCount(); ++i)
		{
			output << "\t( " << (*itr_face)->getEdge(i)->vtx()->x << " , "<< (*itr_face)->getEdge(i)->vtx()->y <<")\t|\t";
		}
		output << "\n";
	}

	output.close();
}

void Triangulation::printSymEdgeList()
{
	printf("\n\n\nPrinting edge list!\n Something is wrong with the g-defuser...\n\n\n");
	for(std::vector<SymEdge*>::iterator it = v_symEdge.begin();
			it != v_symEdge.end();
			++it)
	{
		printf("SYM_EDGE: %d   |   index: %d\nstart: ( %3.0f , %3.0f)   |   end: ( %3.0f , %3.0f)\n\n",
				(int)&(*it),(*it)->vecIndex,(*it)->vtx()->x,(*it)->vtx()->y, (*it)->dst()->x,(*it)->dst()->y);
		fflush(stdout); fflush(stderr);
	}
}

void Triangulation::printRenderQueue(int x, int y)
{
	for (std::vector<Face*>::iterator itr_face = ++(v_face.begin());
			itr_face != v_face.end();
			++itr_face)
		for (int i = 0; i< (*itr_face)->getEdgeCount(); ++i)
			printf("\n%d  |  %d",x,y);
}
#endif

